% 样条拟合数据点

clear;
clc;
% 定义点的矩阵
data = [
120.0798731, 36.2145540;
120.0798738, 36.2145750;
120.0798675, 36.2146510;
120.0798691, 36.2147368;
120.0798703, 36.2148213;
120.0798723, 36.2149135;
120.0798666, 36.2149940;
120.0798696, 36.2150526;
120.0798878, 36.2151376;
120.0799663, 36.2151995;
120.0800706, 36.2152123;
120.0801838, 36.2152091;
120.0803001, 36.2152075;
120.0804170, 36.2152073;
120.0805423, 36.2152265;
120.0806736, 36.2152321;
120.0808106, 36.2152131;
120.0809561, 36.2152573;
120.0811123, 36.2152301;
120.0812173, 36.2152231;
120.0812853, 36.2152213;
120.0813195, 36.2152221;
120.0813926, 36.2152216;
120.0815376, 36.2152218;
120.0817028, 36.2152380;
120.0818716, 36.2152200;
120.0820525, 36.2152473;
120.0822268, 36.2152360;
120.0824110, 36.2152340;
120.0826048, 36.2152435;
120.0827026, 36.2152465;
120.0828165, 36.2151315;
120.0828205, 36.2149925;
120.0828221, 36.2148178;
120.0828221, 36.2146781;
120.0827581, 36.2145310;
120.0826711, 36.2144438;
120.0825228, 36.2144608;
120.0824073, 36.2144546;
120.0822536, 36.2143848;
120.0822430, 36.2142630;
120.0822438, 36.2140925;
120.0822320, 36.2139576;
120.0821196, 36.2138603;
120.0819660, 36.2138743;
120.0817780, 36.2139215;
120.0816141, 36.2138886;
120.0814238, 36.2138956;
120.0813170, 36.2139134;
120.0811468, 36.2138908;
120.0810021, 36.2138916;
120.0808083, 36.2138886;
120.0806713, 36.2138871;
120.0804978, 36.2138565;
120.0804513, 36.2137123;
120.0804206, 36.2135151;
120.0804290, 36.2134026;
120.0804241, 36.2132476;
120.0804116, 36.2131533;
120.0802560, 36.2131256;
120.0800926, 36.2131391;
120.0799135, 36.2130891;
120.0798561, 36.2129658;];

% 地球半径m
R = 6371000; 

% 区分经纬度
longitude = data(:, 1);  
latitude = data(:, 2);   

% 计算与第一个点的经纬度差值
delta_lon = longitude - longitude(1);  
delta_lat = latitude - latitude(1);    

% 转化为m单位坐标矩阵W
delta_x = (delta_lon) * (pi / 180) * R .* cos(deg2rad(latitude));  
delta_y = (delta_lat) * (pi / 180) * R;  
W = [delta_x, delta_y]; 

% 中值滤波
window_size = 3; 
coordinates_filtered = medfilt1(W, window_size);

% 删除离群值
x_filtered = coordinates_filtered(:, 1);
y_filtered = coordinates_filtered(:, 2);
residual_x = delta_x - x_filtered;  
residual_y = delta_y - y_filtered;  
threshold_x = 1 * std(residual_x); 
threshold_y = 1 * std(residual_y); 
outliers_x = abs(residual_x) > threshold_x;
outliers_y = abs(residual_y) > threshold_y;
outliers = outliers_x | outliers_y;  
x_cleaned = x_filtered(~outliers);  % 去除离群点后的x
y_cleaned = y_filtered(~outliers);  % 去除离群点后的y

% 移除重复或接近重复的点
% 计算连续点之间的距离
dist = sqrt(diff(x_cleaned).^2 + diff(y_cleaned).^2);

% 保留距离大于最小阈值的点
min_distance = 1e-6; % 设置小距离阈值以避免数值问题
valid_idx = [true; dist > min_distance]; % 保留第一个点及距离显著的点

% 过滤 x_cleaned 和 y_cleaned
x_cleaned = x_cleaned(valid_idx);
y_cleaned = y_cleaned(valid_idx);
% 参数化数据点：计算累积距离
dist = sqrt(diff(x_cleaned).^2 + diff(y_cleaned).^2); % 相邻点之间的距离diff函数计算相邻只差
cumdist = [0; cumsum(dist)]; % 累积距离

% 使用样条插值拟合
pp_x = spline(cumdist, x_cleaned); % 对x进行样条插值
pp_y = spline(cumdist, y_cleaned); % 对y进行样条插值

% 生成均匀分布的参数值
num_points = 30; % 采样点数量
t_uniform = linspace(0, max(cumdist), num_points);

%获取均匀采样的点
x_fit = ppval(pp_x, t_uniform);
y_fit = ppval(pp_y, t_uniform);

% 均匀采样30个点
num_uniform = 30; % 均匀采样点数量
t_uniform_samples = linspace(0, max(cumdist), num_uniform);
x_uniform = ppval(pp_x, t_uniform_samples);
y_uniform = ppval(pp_y, t_uniform_samples);

% 可视化结果
figure;

% subplot(2, 2, 1);
% plot(x_uniform(:,1), y_uniform(:,2), 'mo--', 'DisplayName', '30个采样点'); % 均匀采样点
% title('程序验证生成点');
% xlabel('X');
% ylabel('Y');

subplot(2,2,1);
plot(delta_x, delta_y, 'bx','DisplayName', '原始数据'); % 原始数据
title('原始数据');
xlabel('X');
ylabel('Y');
legend;

subplot(2, 2, 2);
plot(x_cleaned, y_cleaned, 'bx', 'DisplayName', '去除离群点后数据'); % 去除离群点后数据
title('去除离群点后数据');
xlabel('X');
ylabel('Y');
legend;

subplot(2, 2, 3);
plot(x_fit, y_fit, 'r-', 'DisplayName', '拟合平滑曲线'); % 拟合曲线
title('拟合平滑曲线');
xlabel('X');
ylabel('Y');
legend;

subplot(2, 2, 4);
plot(x_uniform, y_uniform, 'mo--', 'DisplayName', '70个采样点'); % 均匀采样点
title('30个采样点');
xlabel('X');
ylabel('Y');
legend('show');

% 输出均匀采样的点
uniform_points = [x_uniform; y_uniform]';
disp('均匀采样的点：');
disp(uniform_points);